#include "RefFrame.h"

namespace uav
{
namespace nav
{

RefFrame::RefFrame(const GeoCoord &geo) : originGeo(geo)
{
	Fixed sinLat, cosLat, sinLon, cosLon;
	geo.latitude.sinCos(sinLat, cosLat);
	geo.longitude.sinCos(sinLon, cosLon);

	// Compute ECEF coordinates
	Fixed c = sqrt(Fixed::One - WGS84_E2 * (sinLat * sinLat));
	Fixed ac = WGS84_A / c;
	Fixed temp = (ac + geo.altitude) * cosLat;

	originEcef.x = temp * cosLon;
	originEcef.y = temp * sinLon;
	originEcef.z = (ac * WGS84_1_MINUS_E2 + geo.altitude) * sinLat;

	ecefToLocal = Matrix(
		-sinLon,             cosLon,            Fixed::Zero,
		-(sinLat * cosLon), -(sinLat * sinLon), cosLat,
		 cosLat * cosLon,    cosLat * sinLon,   sinLat
	);
}

}}
